#ifndef __WMJ_ROBOT_CONTROL_H__
#define __WMJ_ROBOT_CONTROL_H__
#include <functional>
#include "rclcpp/rclcpp.hpp"
#include "libbase/common.h"
#include "control_base/WMJRobotControl.h"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/float64.hpp"
#include "base_interfaces/msg/gimbal_pose.hpp"
#include "base_interfaces/srv/gimbal_pose.hpp"
#include "base_interfaces/msg/chassis.hpp"
#include "base_interfaces/msg/shooter.hpp"
#include "base_interfaces/msg/communicate.hpp"
#include "base_interfaces/msg/enum.hpp"
#include "base_interfaces/msg/game.hpp"
#include "base_interfaces/srv/trigger.hpp"
// #include "base_interfaces/msg/bt_top.hpp"
#include "base_interfaces/msg/bt_aimer.hpp"
#include "base_interfaces/msg/bt_navigation.hpp"
#include "base_interfaces/msg/scan_ctrl_info.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "std_msgs/msg/char.hpp"
#include <sensor_msgs/msg/joint_state.hpp>
#include <sensor_msgs/msg/imu.hpp> //陀螺仪数据
#include "base_interfaces/msg/big_yaw.hpp"
#include "base_interfaces/msg/bt_judge.hpp"

namespace wmj
{
    using std::placeholders::_1;
    using std::placeholders::_2;

    using namespace std::chrono_literals;
    class Control : public rclcpp::Node
    {
    public:
        int test = 1;
        std::shared_ptr<wmj::WMJRobotControl> RobotControl;
        Control(std::string name, std::shared_ptr<wmj::WMJRobotControl> _RobotControl = nullptr);

        // 云台控制权限 1是Shooter 2是Navigation 3是Scan
        GIMBAL_CONTROL_PERMISSION m_gimbalControlPermissions;

        // 声明云台的回调函数
        // 接收者
        void setGimbalAngleCallback(const base_interfaces::msg::GimbalPose::SharedPtr msg, GIMBAL_ORIENTATION orientation);
        void setGimbalAngleCallbackL(const base_interfaces::msg::GimbalPose::SharedPtr msg);
        void setGimbalAngleCallbackR(const base_interfaces::msg::GimbalPose::SharedPtr msg);
        void setGimbalSpeedCallback(const base_interfaces::msg::GimbalPose::SharedPtr msg, GIMBAL_ORIENTATION orientation = LEFT);
        void setGimbal_YawSpeed_PitchAngle_Callback(const base_interfaces::msg::GimbalPose::SharedPtr msg, GIMBAL_ORIENTATION orientation = LEFT);
        // 发布者
        void publishGimbalAngle();
        void publishGimbalSpeed();
        // 服务端
        void getGimbalAngleTriggerCallback(const base_interfaces::srv::GimbalPose::Request::SharedPtr request,
                                           const base_interfaces::srv::GimbalPose::Response::SharedPtr response);
        void switchTriggerStateCallback(const base_interfaces::srv::Trigger::Request::SharedPtr request,
                                        const base_interfaces::srv::Trigger::Response::SharedPtr response);

        // 声明底盘的回调函数
        // 接收者
        void setTopAndSpeedCallback(const base_interfaces::msg::Chassis::SharedPtr msg);
        void setChassisSpeedCallback(const base_interfaces::msg::Chassis::SharedPtr msg);
        // void setTopCallback(const base_interfaces::msg::BtTop::SharedPtr msg);
        void switchTopStateCallback(const base_interfaces::msg::Chassis::SharedPtr msg);
        // 发布者
        void publishChassisSpeed();
        void publishTopState();

        // 声明射击的回调函数
        // 接收者
        void shootSomeCallback(const base_interfaces::msg::Shooter::SharedPtr msg, GIMBAL_ORIENTATION orientation = LEFT);
        void stopShootCallback(const base_interfaces::msg::Shooter::SharedPtr msg);
        void openBoxCallback(const base_interfaces::msg::Shooter::SharedPtr msg);
        void shootControlCallback(const base_interfaces::msg::BtAimer::SharedPtr msg);
        // 发布者
        void highShootSpeedCallback();
        void getBulletNumberCallback();

        // 声明车间的回调函数
        // 发布者
        void publishRobotMessage();
        // 接受者
        void sendRobotMsgCallback(const base_interfaces::msg::Communicate::SharedPtr msg);

        // 裁判系统通信
        void publishRobotStateMassage();

        // 声明导航的回调函数
        void navigationSubscriptionCallback(const base_interfaces::msg::BtNavigation::SharedPtr msg);
        // 声明扫描的回调函数
        void scanSubscriptionCallback(const base_interfaces::msg::ScanCtrlInfo::SharedPtr msg);
        // 导航目标点
        void publishNavigationGoal(int goal_num);
        void publishNavigationGoalCallback();
        void keySubscription(const std_msgs::msg::Char::SharedPtr msg);

    private:
        // 定义各个通讯端口
        // 云台通讯接口
        rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr m_setGimbalAngleSubscription;
        rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr m_setGimbalAngleSubscriptionL;
        rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr m_setGimbalAngleSubscriptionR;
        rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr m_setGimbalSpeedSubscription;
        rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr m_setGimbal_YawSpeed_PitchAngle_Subscription;
        rclcpp::Subscription<base_interfaces::msg::BtJudge>::SharedPtr    m_BehaviorTree_Subscription;
        rclcpp::Publisher<base_interfaces::msg::GimbalPose>::SharedPtr    m_getGimbalAnglePublisher;
        rclcpp::Publisher<base_interfaces::msg::GimbalPose>::SharedPtr    m_getGimbalSpeedPublisher;
        rclcpp::Service<base_interfaces::srv::GimbalPose>::SharedPtr      m_getGimbalAngleTrigrtService;
        rclcpp::Service<base_interfaces::srv::Trigger>::SharedPtr         m_switchTriggerStateService;

        // 初始化时间函数
        rclcpp::TimerBase::SharedPtr m_getGimbalAnglePublisherTimer;
        rclcpp::TimerBase::SharedPtr m_getGimbalSpeedPublisherTimer;

        // 底盘通讯接口
        rclcpp::Subscription<base_interfaces::msg::Chassis>::SharedPtr m_setTopAndSpeedSubscription;
        rclcpp::Subscription<base_interfaces::msg::Chassis>::SharedPtr m_setChassisSpeedSubscription;
        rclcpp::Subscription<base_interfaces::msg::Chassis>::SharedPtr m_switchTopStateSubscription;
        // rclcpp::Subscription<base_interfaces::msg::BtTop>::SharedPtr   m_setTopSubscription;
        rclcpp::Publisher<base_interfaces::msg::Chassis>::SharedPtr    m_getChassisSpeedPublisher;
        rclcpp::Publisher<base_interfaces::msg::Chassis>::SharedPtr    m_getTopStatePublisher;
        rclcpp::TimerBase::SharedPtr                                   m_getTopStatePublisherTimer;
        rclcpp::TimerBase::SharedPtr                                   m_getChassisSpeedPublisherTimer;

        // 射击通讯接口
        rclcpp::Subscription<base_interfaces::msg::Shooter>::SharedPtr m_shootSomeSubscription;
        rclcpp::Subscription<base_interfaces::msg::Shooter>::SharedPtr m_stopShootSubscription;
        rclcpp::Subscription<base_interfaces::msg::Shooter>::SharedPtr m_openBoxSubscription;
        // ！
        rclcpp::Subscription<base_interfaces::msg::BtAimer>::SharedPtr m_shootControlSubscription;
        rclcpp::Publisher<base_interfaces::msg::Shooter>::SharedPtr    m_highShootSpeedPublisher;
        rclcpp::Publisher<base_interfaces::msg::Shooter>::SharedPtr    m_getBulletNumberPublisher;
        rclcpp::TimerBase::SharedPtr                                   m_highShootSpeedPublisherTimer;
        rclcpp::TimerBase::SharedPtr                                   m_getBulletNumberPublisherTimer;

        // 车间通讯
        rclcpp::Publisher<base_interfaces::msg::Communicate>::SharedPtr    m_recvRobotMsgPublisher;
        rclcpp::TimerBase::SharedPtr                                       m_recvRobotMsgPublisherTimer;
        rclcpp::Subscription<base_interfaces::msg::Communicate>::SharedPtr m_sendRobotMsgSubscription;

        // 裁判系统通信
        rclcpp::Publisher<base_interfaces::msg::Game>::SharedPtr m_gameStatePublisher;
        rclcpp::TimerBase::SharedPtr                             m_gameStatePublishTimer;

        // 导航
        rclcpp::Subscription<base_interfaces::msg::BtNavigation>::SharedPtr m_navigationSubscription;
        // 扫描
        rclcpp::Subscription<base_interfaces::msg::ScanCtrlInfo>::SharedPtr m_scanSubscription;
        // 导航目标点
        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_navigationGoalPublisher;
        rclcpp::TimerBase::SharedPtr                                  m_navigationGoalPublisherTimer;
        rclcpp::Subscription<std_msgs::msg::Char>::SharedPtr          m_keySubscription;

        rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr m_jointStatePublisher;
        float m_last_bullet_speed[2]= {0,0};


    public:
        // 新创建的包
        std::vector<std::shared_ptr<rclcpp::SubscriptionBase>> m_sub_vec;
        std::vector<std::shared_ptr<rclcpp::PublisherBase>> m_pub_vec;
        std::vector<rclcpp::TimerBase::SharedPtr> m_pub_timer_vec;


        void publishGimbalAngle2(int pub_vec_index, GIMBAL_ORIENTATION orientation = LEFT);
        void publishGimbalPoseAngleCallback(int pub_vec_index, GIMBAL_ORIENTATION orientation = LEFT);
        void publishGimbalPoseSpeedCallback(int pub_vec_index, GIMBAL_ORIENTATION orientation = LEFT);
        void publishGimbalRotationRangeCallback(int pub_vec_index, GIMBAL_ORIENTATION orientation = LEFT);
        void setBigYawCallback(const base_interfaces::msg::BigYaw::SharedPtr msg);
        void BehaviorTreeCallBack(const base_interfaces::msg::BtJudge::SharedPtr msg);
        void publishBigYawCallback(int pub_vec_index);
        void publishBulletSpeed(int pub_vec_index, GIMBAL_ORIENTATION orientation = LEFT);

    };
}

#endif //__WMJ_ROBOT_CONTROL_H__